/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */
package wifipositioner;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.HashSet;
import java.util.List;
import java.util.Map;
import org.pi4.locutil.GeoPosition;
import org.pi4.locutil.MACAddress;
import org.pi4.locutil.trace.SignalStrengthSamples;
import org.pi4.locutil.trace.TraceEntry;

/**
 *
 * @author hh
 */



public class FingerPrintingKNN implements IFingerprintWifiPositioner {

    private final int K;
    private final boolean weighted;

    public FingerPrintingKNN(int k, boolean weighted) {
        this.K = k;
        this.weighted = weighted;
    }
    
    @Override
    public ArrayList<PositionCalculationResult> CalculatePositions(List<TraceEntry> offlineTraces, List<TraceEntry> onlineTraces) {
        HashMap<MACAddress, ArrayList<TraceEntry>> index = new HashMap<>();

        ArrayList<PositionCalculationResult> resultList = new ArrayList<>();
        
        // Init HashMap with macadresses as keys and a list of entries that 'sees' the macadress as the value
        for (TraceEntry entry : offlineTraces) {
            SignalStrengthSamples s = entry.getSignalStrengthSamples();

            for (MACAddress m : s.getSortedAccessPoints()) {
                if (index.containsKey(m) == false) {
                    index.put(m, new ArrayList<TraceEntry>());
                }
                index.get(m).add(entry);
            }
        }

        // 
        for (TraceEntry entry : onlineTraces) {
            HashSet<TraceEntry> entries = new HashSet();

            for (MACAddress m : entry.getSignalStrengthSamples().getSortedAccessPoints()) {
                if (index.containsKey(m)) {
                    for (TraceEntry offlineEntry : index.get(m)) {
                        if (entries.contains(offlineEntry) == false) {
                            entries.add(offlineEntry);
                        }
                    }
                }
            }

            double minDistance = Double.MAX_VALUE;

            Map<TraceEntry, Double> KNearestNeighbours = new HashMap<>();

            // Find K nearest neighbours
            for (TraceEntry offline : entries) {
                double distance = DistanceCalculator.calcDistance(offline, entry, 6);

                double largestNeighbourDistance = 0;
                TraceEntry neighbourWithLargestDistance = null;

                for (Map.Entry<TraceEntry, Double> neighbour : KNearestNeighbours.entrySet()) {
                    if (neighbour.getValue() > largestNeighbourDistance) {
                        largestNeighbourDistance = neighbour.getValue();
                        neighbourWithLargestDistance = neighbour.getKey();
                    }
                }

                if ((distance < largestNeighbourDistance || KNearestNeighbours.size() < K) && distance < 1000) {
                    if (neighbourWithLargestDistance != null && KNearestNeighbours.size() >= K) {
                        KNearestNeighbours.remove(neighbourWithLargestDistance);
                    }
                    KNearestNeighbours.put(offline, distance);
                }
            }
            
            // Add the weighted factor to each neighbours value
            double totalDistance = 0;
            for (Map.Entry<TraceEntry, Double> neighbour : KNearestNeighbours.entrySet()) {
                totalDistance += Math.pow(neighbour.getValue(),2);
                neighbour.setValue(1/Math.pow(neighbour.getValue(),2));
            }
            double totalDistanceRelative = 0;
            for (Map.Entry<TraceEntry, Double> neighbour : KNearestNeighbours.entrySet()) {
                double distanceRelative = neighbour.getValue() * totalDistance;
                totalDistanceRelative += distanceRelative;
                neighbour.setValue(distanceRelative);
            }
            for (Map.Entry<TraceEntry, Double> neighbour : KNearestNeighbours.entrySet()) {
                neighbour.setValue(neighbour.getValue()/totalDistanceRelative);
            }

            // Set position based on weighted/unweighted K nearest neigbour
            GeoPosition position = new GeoPosition(0, 0, 0);
            double weightFactor;
            for (Map.Entry<TraceEntry, Double> neighbour : KNearestNeighbours.entrySet()) {
                if(weighted) {
                    weightFactor = neighbour.getValue();
                } else {
                    weightFactor = (double) 1/KNearestNeighbours.size();
                }
                
                position.setX(position.getX() + neighbour.getKey().getGeoPosition().getX() * weightFactor);
                position.setY(position.getY() + neighbour.getKey().getGeoPosition().getY() * weightFactor);
                position.setZ(position.getZ() + neighbour.getKey().getGeoPosition().getZ() * weightFactor);
            }
            
            PositionCalculationResult result = new PositionCalculationResult(position, entry.getGeoPosition());
            resultList.add(result);

            System.out.println("Estimated position: " + position.toStringWithoutOrientation() + " True position: " + entry.getGeoPosition().toString());
        }
        
        return resultList;
    }
}
